#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <string>
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <algorithm>
#include <iostream>

#include "visgrabTypes.h"

using namespace std;
using boost::property_tree::ptree;

class GraspSuccess {
public:
  void print() { cout << "GraspSuccess, gwsQuality: " << gwsQuality << endl; };
  double gwsQuality;
};

class LiftSuccess {
public:
  void print() { 
    cout << "LiftSuccess, gwsQuality: " << gwsQuality << endl; 
    cout << "LiftSuccess, liftQuality: " << liftQuality << endl;
  };
  double gwsQuality;
  double liftQuality;
};

class GraspResult {
public:
  GraspResult() {
    graspSuccess = NULL;
    liftSuccess = NULL;
  };
  ~GraspResult() {
    if(graspSuccess) delete graspSuccess;
    if(liftSuccess) delete liftSuccess;
  };
  void setGraspSuccessGWSQuality(double v) {
    if(graspSuccess == NULL)
      graspSuccess = new GraspSuccess;
    graspSuccess->gwsQuality = v;
  };
  void setLiftSuccessGWSQuality(double v) {
    if(liftSuccess == NULL)
      liftSuccess = new LiftSuccess;
    liftSuccess->gwsQuality = v;
  };
  void setLiftSuccessLiftQuality(double v) {
    if(liftSuccess == NULL)
      liftSuccess = new LiftSuccess;
    liftSuccess->liftQuality = v;
  };
  void print() {
    cout << "graspResult: " << graspResult << endl;
    cout << "graspResultID: " << graspResultID << endl;
    if(graspSuccess)
      graspSuccess->print();
    if(liftSuccess)
      liftSuccess->print();
  };
  int graspID;
  string graspResult;
  int graspResultID; 
  GraspSuccess* graspSuccess;
  LiftSuccess* liftSuccess;
};

/*!
 * Reading the scene info from the manifest XML file. The function takes
 * the ID from the specific experiment and searches for the scene info
 * in the manfifest file. 
 * @param ptMan The ptree containing the complete manifest file.
 * @param ptResExp The ptree containing one of the the 'experiment' fields.
 * @return A pointer to a SceneInfo object.
 */
SceneInfo* readSceneInfo(ptree &ptMan, ptree &ptResExp) {
  SceneInfo* sceneInfo = NULL;

  int expID = ptResExp.get<int>("<xmlattr>.id");
  
  // Find the scene with the ID matching the expID
  BOOST_FOREACH(ptree::value_type &v, ptMan.get_child("visgrab.benchmark")) {
    if((string)v.first.data() == "scene") {
      ptree ptManScene = v.second;
      int sceneID = ptManScene.get<int>("<xmlattr>.id");
      if(sceneID == expID) {
	sceneInfo = new SceneInfo;
	sceneInfo->sceneLocator = ptManScene.get<string>("info.sceneLocator");
	sceneInfo->textureCondition = ptManScene.get<string>("info.textureCondition");
	sceneInfo->poseCondition = ptManScene.get<string>("info.poseCondition");
	sceneInfo->sceneSet = ptManScene.get<string>("info.sceneSet");
	sceneInfo->sceneSetID = ptManScene.get<int>("info.sceneSetID");
	sceneInfo->poseID = ptManScene.get<int>("info.poseID");
	sceneInfo->nrObjects = ptManScene.get<int>("info.nrObjects");
	BOOST_FOREACH(ptree::value_type &v2, ptManScene.get_child("info.objects")) {
	  if((string)v2.first.data() == "object") {
	    sceneInfo->objects.push_back( v2.second.data() );
	  }
	}
      }      
    } 
  }

  if(sceneInfo == NULL) {
    cout << "ERROR: scene with ID " << expID << " could not be found in the manifest file" << endl;
    exit(EXIT_FAILURE);
  }

  return sceneInfo;
}

/*!
 * Reading the grasp info from the XML file. 
 * @param pt The ptree containing one of the 'result' fields.
 * @return A pointer to a GraspResult object.
 */
GraspResult* readGraspResult(ptree &pt) {
  GraspResult* graspResult = new GraspResult;
  graspResult->graspID = pt.get<int>("<xmlattr>.graspID");
  graspResult->graspResult = pt.get<string>("graspResult");
  graspResult->graspResultID = pt.get<int>("graspResultID");
  if(graspResult->graspResultID == 1 || graspResult->graspResultID == 4 || graspResult->graspResultID == 5 ) {
    graspResult->setGraspSuccessGWSQuality(pt.get<double>("graspSuccess.gwsQuality"));
  }
  if(graspResult->graspResultID == 1 || graspResult->graspResultID == 5 ) {
    graspResult->setLiftSuccessGWSQuality(pt.get<double>("liftSuccess.gwsQuality"));
    graspResult->setLiftSuccessLiftQuality(pt.get<double>("liftSuccess.liftQuality"));
  }

  return graspResult;
}

/*!
 * Checks if the directory 'dir' exists and makes the directory
 * if this is not the case. 
 * NB. This only works if the parent directory exists.
 * @param dir The directory name that needs to be made. 
 */
void makeDir(string dir) {
    struct stat stInfo;
    if (stat(dir.c_str(), &stInfo) != 0) {
      system( ("mkdir " + dir).c_str());      
    }
}

/*!
 * Checks if the vector of strings contains a specific string.
 * @param str The string that is searched for in the vector.
 * @param vec The vecotr of strings that is searched.
 */
bool inVector(string str, vector<string> &vec) {
  for(unsigned int i=0; i<vec.size(); i++) {
    if(vec[i] == str)
      return true;
  }
  return false;
}

/*!
 * Writes the header for the data file, i.e., writes the names of 
 * the columns.
 */
void writeDatHeading(FILE* fp) {
  fprintf(fp, "textureConditionID\t");
  fprintf(fp, "nrObjects\t");
  fprintf(fp, "poseConditionID\t");
  fprintf(fp, "sceneSetID\t");
  fprintf(fp, "poseID\t");
  fprintf(fp, "graspID\t");
  fprintf(fp, "graspResult\t");
  fprintf(fp, "graspGWSQuality\t");
  fprintf(fp, "liftGWSQuality\t");
  fprintf(fp, "liftQuality\n");
}

/*!
 * Writes the grasp result to a data file.
 * @param fp A file pointer to the data file.
 * @param sceneInfo A pointer to the scene information.
 * @param graspResult A pointer to the grasp results.
 * @param nrData A counter keeping track of the number of rows
 * written in the data file.
 */
void writeGraspResult(FILE* fp, SceneInfo* sceneInfo, GraspResult* graspResult, int &nrData) {
  if(sceneInfo->textureCondition == "textured")
    fprintf(fp, "0\t");
  else
    fprintf(fp, "1\t");
  fprintf(fp, "%d\t", sceneInfo->nrObjects);
  if(sceneInfo->nrObjects == 1) {
    if(sceneInfo->poseCondition == "standing")
      fprintf(fp, "0\t");
    else
      fprintf(fp, "1\t");
  }
  else {
    if(sceneInfo->poseCondition == "far")
      fprintf(fp, "0\t");
    else
      fprintf(fp, "1\t");
  }
  fprintf(fp, "%d\t", sceneInfo->sceneSetID);
  fprintf(fp, "%d\t", sceneInfo->poseID);
  fprintf(fp, "%d\t", graspResult->graspID);
  fprintf(fp, "%d\t", graspResult->graspResultID);
  if(graspResult->graspSuccess)
    fprintf(fp, "%f\t", graspResult->graspSuccess->gwsQuality);
  else
    fprintf(fp, "0\t");
  if(graspResult->liftSuccess)
    fprintf(fp, "%f\t", graspResult->liftSuccess->gwsQuality);
  else
    fprintf(fp, "0\t");
  if(graspResult->liftSuccess)
    fprintf(fp, "%f\n", graspResult->liftSuccess->liftQuality);
  else
    fprintf(fp, "0\n");
  nrData++;
}

/*!
 * Creates the data file datFile, writes the heading and returns the file pointer.
 * @param datFile The name of the data file that needs to be created.
 * @return The file pointer.
 */
FILE* openDatFile(string datFile) {
  FILE *fp = fopen(datFile.c_str(), "w+");
  if(!fp) {
    cout << "ERROR: could not open file to write: " << datFile << endl;
    exit(0);
  }
  writeDatHeading(fp);
  return(fp);
}

/*!
 * Closes the opened data file. 
 * @param fp The file pointers to the data file that is opened.
 * @param datFile The names of the data file.
 * @param nrData The number of rows written to the data file.
 * @param success A boolean describing whether all data has been extracted from the XML file.
 */
void closeFP(FILE* fp, string datFile, int nrData, bool success) {
  fclose(fp);
  cout << "Written " << nrData << " rows to " << datFile << endl;
  if(!success)
    cout << "INCOMPLETE" << endl;
}


/*!
 * readResults
 * The program reads the XML result file that is provided by the RoBWork
 * grasp simulation. Information about the results is combined with 
 * information about the scenes, found in the visgrab-manifest file. 
 * All results are extracted and written to a Matlab data file. This
 * data file can be used with the Matlab function 'visgrabAnalyzeResults.m'
 * to create latex/html tables, and/or plots.
 *
 * Usage: readResults <VisgrabManifest.xml> <VisgrabResults.xml> <matlab_data_file>.
 *   VisgrabManifest.xml  The Visgrab manifest XML file
 *   VisgrabResults.xml   The Visgrab result XML file
 *   matlab_data_file     The Matlab data file 
 */
int main(int argc, char *argv[]) {

  if(argc < 4) {
    cout << "Usage: readResults <VisgrabManifest.xml> <VisgrabResults.xml> <DataFile.dat>" << endl;
    return EXIT_FAILURE;
  }

  string manifestFile = (string)argv[1];
  string resultFile = (string)argv[2];
  string matlabFile = (string)argv[3];

  using boost::filesystem::path;
  path manifestPath(manifestFile);
  path resultPath(resultFile);

  if(!exists(manifestPath)) {
    std::cout << "Manifest file does not exist" << std::endl;
    exit(EXIT_FAILURE);
  }
  if(!exists(resultPath)) {
    std::cout << "Result file does not exist" << std::endl;
    exit(EXIT_FAILURE);
  }

  if(extension(manifestPath)!=".xml") {
    std::cout << "Manifest file should be an XML file" << std::endl;
    exit(EXIT_FAILURE);
  }
  if(extension(resultPath)!=".xml") {
    std::cout << "Result file should be an XML file" << std::endl;
    exit(EXIT_FAILURE);
  }


  cout << "Reading manifest file: " << manifestFile << endl << endl;
  ptree ptMan;
  read_xml(manifestFile, ptMan, boost::property_tree::xml_parser::trim_whitespace);

  cout << "Reading result file: " << resultFile << endl << endl;
  ptree ptRes;
  read_xml(resultFile, ptRes, boost::property_tree::xml_parser::trim_whitespace);

  FILE* fp;
  int nrData= 0;
  fp = openDatFile(matlabFile);

  BOOST_FOREACH(ptree::value_type &v, ptRes.get_child("visgrab")) {
    if((string)v.first.data() == "experiment") {
      ptree ptResExp = v.second;
      SceneInfo* sceneInfo = readSceneInfo(ptMan, ptResExp);

      BOOST_FOREACH(ptree::value_type &v2, ptResExp) {
	if((string)v2.first.data() == "result") {
	  ptree ptResGrasp = v2.second;
	  GraspResult* graspResult = readGraspResult(ptResGrasp);
	  if(graspResult != NULL)
	    writeGraspResult(fp, sceneInfo, graspResult, nrData);
	  else { // Error occurred during reading of the grasp result
	    closeFP(fp, matlabFile, nrData, false);
	    return(EXIT_FAILURE);
	  }
	  delete graspResult;
	}
      }
      delete sceneInfo;
    }
  }
  closeFP(fp, matlabFile, nrData, true);

  return(EXIT_SUCCESS);
}
